import numpy as np
import time as t
from robotPi import robotPi
import cv2
import os
#from rev_cam import rev_cam
import ctypes
from ctypes import *

speed=50

def unauto_pilot():
    global speed
    global t
    speed=50
    robot = robotPi()
    #robot.movement.set_volume(0)                                                                                                                                                   command = 0
    cap = cv2.VideoCapture(0)
    
    c=t.time()
    t=t.ctime(c)
    videoname=str(t)+'.avi'
    fourcc=cv2.VideoWriter_fourcc('M','P','4','2')
    out=cv2.VideoWriter(videoname,fourcc,10,(640,480))                                                                                                                                                                                                                 
    timeStart = cv2.getTickCount()
    while 1:
        timePresent = cv2.getTickCount()
        time = (timePresent - timeStart) / cv2.getTickFrequency() * 1000
        _, frame = cap.read()
        #frame = rev_cam(frame)
        out.write(frame)
        resized_height = int(480 * 0.75)
        # 计算缩放比例                                                                                                                                    
        frame = cv2.resize(frame, (480, 180))
        frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        # slice the lower part of a frame
        res = frame[:, :]                                                                                         
        cv2.imshow("review", res)
        ccommand = 0
        ccommand = cv2.waitKey(100) & 0xFF
        if ccommand == ord('q'):                             
            break
        elif ccommand == ord('w'):                              
            robot.movement.move_forward(30,300)
        elif ccommand == ord('a'):
            robot.movement.move_free(0, 30, 80, 400)
            print('[%d]left'%time)
        elif ccommand == ord('d'):
            robot.movement.move_free(0, 30, -80, 400)
            print('[%d]right'%time)
        # stop-sign -- 3
        elif ccommand == ord('s'):
            robot.movement.move_free(0, 0, 0, 200)
        else:
            robot.movement.move_forward(30,300)
            print('[%d]forward'%time)
    print('[%d]detect stop line & hit'%time)
    robot.movement.hit()
    cv2.waitKey(500) 
    cap.release()
    cv2.destroyAllWindows()
      

if __name__ == '__main__':
    
    unauto_pilot()
    robot = robotPi()
    robot.movement.set_volume(0)
    #robot.movement.hit()
